clear
a = [0 1 1]';
a = normalize(a,'norm');
theta_vi = 1;
% C_vi = cos(theta_vi)*eye(3)+(1-cos(theta_vi))*a*a'-sin(theta_vi)*so3(a) %true convention

C_iv = cos(theta_vi)*eye(3)+(1-cos(theta_vi))*a*a' + sin(theta_vi)*so3(a) %common in robotics

SO3 = expm(so3(theta_vi*a))

